{
 "cells": [
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "import os\n",
    "import numpy\n",
    "from numpy import zeros, arange, nan, real, imag, pi, sin, arctan, arctan2, angle, exp, sqrt, diff, pad, std\n",
    "from scipy.io import loadmat, savemat\n",
    "from gps_l1ca_utils import generate_GPS_L1CA_code\n",
    "from acquisition_utils import coarse_acquire\n",
    "from utils import PSKSignal, sample_sequence\n",
    "from tracking_utils import compute_correlations"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Choose IF data file and appropriate data parameters\n",
    "filepath = '../data/sim-RF_GPS-L1CA_5000_1250_complex_06s.mat'\n",
    "data_type = 'simulated'\n",
    "fs = 5e6                        # sampling rate (Hz)\n",
    "f_center = 1.57542e9 - 1.25e6   # radio front-end center frequency (Hz)\n",
    "prns = [4, 7, 10, 15, 29, 32]\n",
    "prn = 10\n",
    "\n",
    "# filepath = '../data/20190129_225529_gpsl1_tcxo_5000_0_4bit.mat'\n",
    "# data_type = 'real-tcxo'\n",
    "# fs = 5e6                        # sampling rate (Hz)\n",
    "# f_center = 1.57542e9            # radio front-end center frequency (Hz)\n",
    "# prns = [4, 5, 7, 8, 9, 16, 23, 27, 28, 30]\n",
    "# prn = 4\n",
    "\n",
    "# filepath = '../data/20190129_230020_gpsl1_ocxo_5000_0_4bit.mat'\n",
    "# data_type = 'real-ocxo'\n",
    "# fs = 5e6                        # sampling rate (Hz)\n",
    "# f_center = 1.57542e9            # radio front-end center frequency (Hz)\n",
    "# prns = [4, 5, 7, 8, 9, 16, 23, 27, 28, 30]  # maybe 11\n",
    "# prn = 4\n",
    "\n",
    "# Load IF samples\n",
    "IF_data = loadmat(filepath)\n",
    "samples = IF_data['samples'][0]\n",
    "T_data = len(samples) / fs      # duration of signal (s)\n",
    "\n",
    "# Set parameters for GPS L1CA signal\n",
    "f_carrier = 1.57542e9           # L1 carrier frequency (Hz)\n",
    "f_code = 1.023e6                # L1 C/A code rate (chips/s)\n",
    "f_inter = f_carrier - f_center  # intermediate frequency (Hz)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Create signal and acquire\n",
    "signal = PSKSignal(generate_GPS_L1CA_code(prn), 1.023e6, f_carrier)\n",
    "doppler_bins = arange(-4000, 4000, 5)\n",
    "coarse_acq_results = coarse_acquire(samples, fs, f_center, signal, doppler_bins, int(8e-3 * fs), 2, True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Set tracking loop parameters\n",
    "T_blk = 2e-3                        # tracking block duration / integration period (s)\n",
    "N_blk = int(T_blk * fs)             # number of samples per block\n",
    "N_blocks = len(samples) // N_blk    # total number of blocks in the data\n",
    "delay_spacing = 0.5                 # DLL correlator delay spacing\n",
    "# Define tracking loop bandwidths\n",
    "B_DLL = 2\n",
    "B_PLL = 20"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Preallocate outputs\n",
    "outputs = {\n",
    "    key: nan * zeros(N_blocks) for key in [\n",
    "        'code_phase',\n",
    "        'measured_code_phase',\n",
    "        'filtered_code_phase',\n",
    "        'carrier_phase',\n",
    "        'measured_carrier_phase',\n",
    "        'filtered_carrier_phase',\n",
    "        'doppler_freq',\n",
    "        'measured_doppler_freq',\n",
    "        'filtered_doppler_freq',\n",
    "    ]\n",
    "}\n",
    "for key in ['early', 'prompt', 'late']:\n",
    "    outputs[key] = nan * zeros(N_blocks, dtype=complex)\n",
    "outputs['prn'] = prn\n",
    "outputs['fs_IF'] = fs\n",
    "outputs['f_center_IF'] = f_center\n",
    "outputs['acq_correlation'] = coarse_acq_results['correlation']\n",
    "outputs['acq_doppler_bins'] = doppler_bins\n",
    "outputs['acq_snr'] = coarse_acq_results['snr']\n",
    "outputs['n0'] = coarse_acq_results['n0']\n",
    "outputs['code_phase0'] = coarse_acq_results['code_phase']\n",
    "outputs['doppler_freq0'] = coarse_acq_results['doppler_freq']\n",
    "outputs['time'] = arange(N_blocks) * T_blk\n",
    "outputs['B_PLL'] = B_PLL\n",
    "outputs['B_DLL'] = B_DLL\n",
    "outputs['T'] = T_blk"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Set tracking state variables to coarse acquisition results\n",
    "code_phase = coarse_acq_results['code_phase']\n",
    "carrier_phase = 0\n",
    "doppler_freq = coarse_acq_results['doppler_freq']\n",
    "\n",
    "# Run tracking loop\n",
    "for i in range(N_blocks): \n",
    "    # Get i-th block of samplescode_phase\n",
    "    block = samples[i * N_blk:(i + 1) * N_blk]\n",
    "    block -= numpy.mean(block)  # remove any DC bias\n",
    "    \n",
    "    # Obtain early, prompt, and late correlator outputs\n",
    "    tracking_state = (code_phase, carrier_phase, doppler_freq)\n",
    "    early, prompt, late = compute_correlations(block, fs, f_center, signal, tracking_state, [delay_spacing, 0, -delay_spacing])\n",
    "    \n",
    "    ### DLL ###\n",
    "    # 1) Compute code phase error using early-minus-late discriminator\n",
    "    code_phase_error = delay_spacing * (abs(early) - abs(late)) / (abs(early) + abs(late) + 2 * abs(prompt))\n",
    "    \n",
    "    # 2) Filter code phase error to reduce noise\n",
    "    #  We implement the DLL filter by updating code phase in proportion to code\n",
    "    #  phase dicriminator output.  The result has the equivalentresponse of a\n",
    "    #  1st-order DLL filter\n",
    "    filtered_code_phase_error = T_blk * B_DLL / .25 * code_phase_error\n",
    "    \n",
    "    measured_code_phase = code_phase + code_phase_error\n",
    "    filtered_code_phase = code_phase + filtered_code_phase_error\n",
    "    \n",
    "    ### PLL ###\n",
    "    # 1) Compute phase error (in cycles) using appropriate phase discriminator\n",
    "    delta_theta = arctan(imag(prompt) / real(prompt)) / (2 * pi)\n",
    "#     delta_theta = arctan2(imag(prompt), real(prompt)) / (2 * pi)\n",
    "    carrier_phase_error = delta_theta\n",
    "    doppler_freq_error = T_blk / 2 * delta_theta\n",
    "    \n",
    "    # 2) Filter carrier phase error to reduce noise\n",
    "    #  We implement the PLL filter by updating carrier phase and frequency in\n",
    "    #  proportion to the phase discriminator output in a way that has the\n",
    "    #  equivalent response to a 2nd-order PLL filter\n",
    "    zeta = 1 / sqrt(2)\n",
    "    omega_n = B_PLL / .53\n",
    "    filtered_carrier_phase_error = (2 * zeta * omega_n * T_blk - 3 / 2 * omega_n**2 * T_blk**2) * delta_theta\n",
    "    filtered_doppler_freq_error = omega_n**2 * T_blk * delta_theta\n",
    "    \n",
    "    measured_carrier_phase = carrier_phase + carrier_phase_error\n",
    "    filtered_carrier_phase = carrier_phase + filtered_carrier_phase_error\n",
    "    \n",
    "    measured_doppler_freq = doppler_freq + doppler_freq_error\n",
    "    filtered_doppler_freq = doppler_freq + filtered_doppler_freq_error\n",
    "    \n",
    "    # Write outputs\n",
    "    outputs['early'][i] = early\n",
    "    outputs['prompt'][i] = prompt\n",
    "    outputs['late'][i] = late\n",
    "    outputs['code_phase'][i] = code_phase\n",
    "    outputs['measured_code_phase'][i] = measured_code_phase\n",
    "    outputs['filtered_code_phase'][i] = filtered_code_phase\n",
    "    outputs['carrier_phase'][i] = carrier_phase\n",
    "    outputs['measured_carrier_phase'][i] = measured_carrier_phase\n",
    "    outputs['filtered_carrier_phase'][i] = filtered_carrier_phase\n",
    "    outputs['doppler_freq'][i] = doppler_freq\n",
    "    outputs['measured_doppler_freq'][i] = measured_doppler_freq\n",
    "    outputs['filtered_doppler_freq'][i] = filtered_doppler_freq\n",
    "    \n",
    "    # Update to next time epoch (this step is considered part of the loop filter!)\n",
    "    code_phase = filtered_code_phase\n",
    "    carrier_phase = filtered_carrier_phase\n",
    "    doppler_freq = filtered_doppler_freq\n",
    "    \n",
    "    #  Here we apply carrier-aiding by adjusting `f_code` based on Doppler frequency\n",
    "    f_code_adj = signal.f_code * (1 + doppler_freq / signal.f_carrier)\n",
    "    code_phase += f_code_adj * T_blk\n",
    "    f_inter = signal.f_carrier - f_center\n",
    "    carrier_phase += (f_inter + doppler_freq) * T_blk"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# Remove nominal code rate and intermediate frequency from outputs\n",
    "#  This makes it easier to:\n",
    "#    1) compare code phase to a real receiver's pseudorange outputs\n",
    "#    2) compare carrier phase output between datasets with different intermediate frequencies\n",
    "t = arange(N_blocks) * T_blk\n",
    "outputs['code_phase'] -= t * f_code\n",
    "outputs['measured_code_phase'] -= t * f_code\n",
    "outputs['filtered_code_phase'] -= t * f_code\n",
    "outputs['carrier_phase'] -= t * f_inter\n",
    "outputs['measured_carrier_phase'] -= t * f_inter\n",
    "outputs['filtered_carrier_phase'] -= t * f_inter"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "# If data was simulated, we should also store the true state values\n",
    "if data_type == 'simulated':\n",
    "    prns = IF_data['prns'][0]\n",
    "    chips = IF_data['chips']\n",
    "    chips = dict(zip(prns, chips))\n",
    "    code_phase_truth = (chips[prn][::N_blk])[:N_blocks]\n",
    "    carrier_phase_truth = code_phase_truth * f_carrier / signal.f_code\n",
    "    carrier_phase_truth -= carrier_phase_truth[0] - carrier_phase_truth[0] % 1\n",
    "    doppler_freq_truth = pad(diff(carrier_phase_truth), (0, 1), 'edge') / T_blk\n",
    "\n",
    "    outputs['code_phase_truth'] = code_phase_truth\n",
    "    outputs['carrier_phase_truth'] = carrier_phase_truth\n",
    "    outputs['doppler_freq_truth'] = doppler_freq_truth"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": [
    "output_dir = '../tracking-output/'\n",
    "if not os.path.exists(output_dir):\n",
    "    os.makedirs(output_dir)\n",
    "output_filepath = os.path.join(output_dir, '{0}_PRN-{1:02}_PLL-BW-{2:02}.mat'.format(data_type, prn, B_PLL))\n",
    "savemat(output_filepath, outputs)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.7.1"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}
